#include <stdio.h>
#include "FreeRTOS.h"

#include "task.h"

#include "ti_msp_dl_config.h"

#include "encoder.h"
#include "as5047p.h"
#include "math.h"


#define ride_cm    6 // 轮子直径    差速轮子直径为6.5cm
    // 电机轴一圈11个脉冲，减速比30，所以电机轴30圈，轮子转1圈，
    // 脉冲数30*11=330，周长20.42，每cm脉冲数，330/20.42 = 16.16个脉冲



// long g_lMotorPulseSigma = 0;
// short g_1nMotorPulse = 0; // 全局变量， 保存电机脉冲数值
// as5047p_handle_t as5047p;

// void as5047p_spi_send(uint16_t data);
// uint16_t as5047p_spi_read(void);
// void as5047p_spi_select(void);
// void as5047p_spi_deselect(void);
// void as5047p_delay(void);

// void as5047p_init() {
//   as5047p_make_handle(&as5047p_spi_send, &as5047p_spi_read, &as5047p_spi_select,
//                       &as5047p_spi_deselect, &as5047p_delay, &as5047p);
// }

// void as5047p_spi_send(uint16_t data) {
//   // uint8_t tx_buf[2] = {(uint8_t)(data >> 8), (uint8_t)data};
//   // DL_SPI_transmitData16(SPI_encoder_INST, *(uint16_t *)(tx_buf));
//   DL_SPI_transmitData16(SPI_encoder_INST, data);
// }

// uint16_t as5047p_spi_read(void) {
//   uint16_t res = DL_SPI_receiveDataBlocking16(SPI_encoder_INST);
//   // printf("\tread: %d\r\n", (int)res);
//   return res;
// }

// void as5047p_spi_select(void) {
//   DL_GPIO_clearPins(GPIO_SPI_encoder_CS0_PORT, GPIO_SPI_encoder_CS0_PIN);
// }

// void as5047p_spi_deselect(void) {

//   DL_GPIO_setPins(GPIO_SPI_encoder_CS0_PORT, GPIO_SPI_encoder_CS0_PIN);
// }

// void as5047p_delay(void) {
//   vTaskDelay(pdMS_TO_TICKS(1));
// }

// float angle_diff(float new_angle, float last_angle) {
//   float diff = new_angle - last_angle;
//   return remainder(diff, 360); // 直接映射到 [-180, 180]
// }
